#ifndef PIXELTRANSFORM_H
#define PIXELTRANSFORM_H

#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <image_transport/image_transport.h>
#include <image_geometry/pinhole_camera_model.h>

#include <cv_bridge/cv_bridge.h>

/*
  The purpose of the PixelTransform class is to provide transformations between pixel coordinates and base coordinates,
  using depth information from the kinect and the /base_link to /camera_link tf transform (see the void_tf package).

  The PixelTransform class is used by the ball detect, line detect and robot detect modules to find real world coordinates of
  objects.

  For depth perception to work properly, depth registration needs to be activated in dynamic_reconfigure.
  */
class PixelTransform
{
public:
	// Needs to be created after ros::init. Will subscribe to "camera/depth_registered/image_rect"
	PixelTransform(ros::NodeHandle& node);

	// Must call this for every camara info package received
	void UpdateModel(const sensor_msgs::CameraInfoConstPtr &camInfo);

	// Transform one pixel to an xyz point in the base frame (base_link). Depends on the kinect transform being in the system
	// Will return true if the point has been converted to base_point, and false otherwise
	bool transformFromPixelToBase(cv::Point2d point, geometry_msgs::Point &base_point);

	bool transformFromPixelTo3d(cv::Point2d xypoint, cv::Point3d& xyzpoint);
	bool transformFrom3dToBase(cv::Point3d xyzpoint, geometry_msgs::Point &base_point);
	cv::Point2d transformFrom3dToPixel(cv::Point3d xyzpoint);

	// find the depth at a pixel, return true if there was a depth value there
	bool Depth(cv::Point2d xyPoint, float &depth);
private:
	tf::TransformListener mTfListener;

	image_geometry::PinholeCameraModel mCameraModel;

	ros::Subscriber sub_depth;

	static void pixelTransformDepthCallback(const sensor_msgs::ImageConstPtr &msg);
	static bool hasDepthImage;
	static cv_bridge::CvImagePtr depth_cv_ptr;
};

#endif // PIXELTRANSFORM_H
